function localSequence=optimizeControlPoints(tour1, tour2, d,prev, after, ...
    lambda, localSequence, correspondingTriangles, sites, triangles)

N=length(correspondingTriangles);
delta=10; %could reduce if it does not converge
maxZ=max(max(sites(:,3)), max(tour1(3), tour2(3)));
maxZ=maxZ+delta+d;

normSum=0;
for i=2:(N-2)
    normSum=normSum+norm(localSequence(:,i)-localSequence(:,i+1));
end
t_prev=delta*(N-3)/normSum;
t_after=t_prev;

if ~isempty(prev)
    correspondingTriangle(1:3)=0;
end
if ~isempty(after)
    correspondingTriangle((N-2):N)=0;
end

cvx_begin
    variable X(3*N)
    variable k
    variable S(2*N)
    
    % minimize maximum curvature
    minimize (k+lambda*norm(S))
    
    subject to
        for i=2:2:(N-3)
            norm(X((3*i-2):(3*i))-2*X((3*i+1):(3*i+3))+X((3*i+4):(3*i+6)))<=k
        end
    
        % constraint1: control points are above the corresponding triangles
        % also keep the X,Y coordinates of the points closer to initial
        % guess
        for i=1:N
            if correspondingTriangles(i)
                [normal, offset]=planeCoeff(triangles(correspondingTriangles(i),:), sites);
                normal*X((3*i-2):(3*i))>=offset;
                normal*X((3*i-2):(3*i))<=offset+maxZ;
                S((2*i-1):(2*i))==X((3*i-2):(3*i-1))-localSequence(1:2,i);
            else
                S((2*i-1):(2*i))==0;
            end
        end
        S<=2e5;
        S>=-2e5;
        
        % constraint2: end points.
        % 2.(a) if prev or after is not empty, fix the value
        % 2.(b) if prev or after is empty, pass within +-d bounding box and
        % length delta
        if ~isempty(prev)
            prev==X(7:9);
            tour1==X(4:6);
        else
            X(4:6)<=tour1+d;
            X(4:6)>=tour1-d;
            %norm(X(1:3)-X(4:6))<=delta;
        end
        if ~isempty(after)
            after==X((3*N-8):(3*N)-6);
            tour2==X((3*N-5):(3*N-3));
        else
            X((3*N-5):(3*N-3))<=tour2+d;
            X((3*N-5):(3*N-3))>=tour2-d;    
            %norm(X((3*N-5):(3*N-3))-X((3*N-2):(3*N)))<=delta;
        end
        
        %constraint3: C1 constraints
        if isempty(prev)
            (1+t_prev)*X(4:6)==X(1:3)+t_prev*X(7:9);
        end
        if isempty(after)
            (1+t_after)*X((3*N-5):(3*N-3))==X((3*N-2):(3*N))+t_after*X((3*N-8):(3*N-6));
        end
        for i=4:2:(N-3)
            2*X((3*i-2):(3*i))==X((3*i-5):(3*i-3))+X((3*i+1):(3*i+3));
        end
            
cvx_end  

if strcmp(cvx_status, 'Solved')
    localSequence=reshape(X, 3, N);
else
    localSequence=[];
end

end

function [a, d]=planeCoeff(triangle, sites)
A=sites(triangle(1), :);
B=sites(triangle(2), :);
C=sites(triangle(3), :);
a=cross(B-A, C-A);
a=a./a(3);
if a(3)<0
    a=-a;
end
d=a*A';

end